__author__ = 'YT'

from .component import map
from . import tool

# 定义一个SearchEntry类
class SearchEntry():
    # 初始化函数
    def __init__(self, x, y, g_cost, f_cost=0, pre_entry=None):
        # x坐标
        self.x = x
        # y坐标
        self.y = y
        # g_cost
        self.g_cost = g_cost
        # f_cost
        self.f_cost = f_cost
        # pre_entry
        self.pre_entry = pre_entry
    
    def getPos(self):
        '''返回一个元组，包含机器人的坐标信息'''
        return (self.x, self.y)


def AStarSearch(map, source, dest):
    '''
    A* 搜索算法
    '''
    def getNewPosition(map, locatioin, offset):
        '''
        根据当前位置和偏移量获取新的位置
        '''
        x,y = (location.x + offset[0], location.y + offset[1])
        if not map.isValid(x, y) or not map.isMovable(x, y):
            return None
        return (x, y)
    def getPositions(map, location):
        '''
        根据当前位置获取可以移动的所有位置
        '''
        offsets = tool.getMovePositions(location.x, location.y)
        poslist = []
        for offset in offsets:
            pos = getNewPosition(map, location, offset)
            if pos is not None:
                poslist.append(pos)
        return poslist
    
    # imporve the heuristic distance more precisely in future
    def calHeuristic(map, pos, dest):
        # 计算启发式距离
        return map.calHeuristicDistance(dest.x, dest.y, pos[0], pos[1])
        
    def getMoveCost(location, pos):
        # 计算移动成本
        if location.x != pos[0] and location.y != pos[1]:
            # 如果x和y坐标都不同，则成本为1.4
            return 1.4
        else:
            # 如果x和y坐标有一个相同，则成本为1
            return 1

    # check if the position is in list
    def isInList(list, pos):
        if pos in list:
            return list[pos]
        return None
    
    def addAdjacentPositions(map, location, dest, openlist, closedlist):
        # 获取当前位置可以移动的所有位置
        poslist = getPositions(map, location)
        for pos in poslist:
            # 如果位置已经在closedlist中，则不操作
            if isInList(closedlist, pos) is None:
                findEntry = isInList(openlist, pos)
                # 计算启发式成本
                h_cost = calHeuristic(map, pos, dest)
                # 计算移动成本
                g_cost = location.g_cost + getMoveCost(location, pos)
                if findEntry is None :
                    # 如果位置不在openlist中，则将其添加到openlist中
                    openlist[pos] = SearchEntry(pos[0], pos[1], g_cost, g_cost+h_cost, location)
                elif findEntry.g_cost > g_cost:
                    # 如果位置在openlist中，且成本大于当前成本，
                    # 则更新成本和前一个位置
                    findEntry.g_cost = g_cost
                    findEntry.f_cost = g_cost + h_cost
                    findEntry.pre_entry = location

    
    # find a least cost position in openlist, return None if openlist is empty
    def getFastPosition(openlist):
        # 定义一个变量fast，用来保存openlist中f_cost最小的节点
        fast = None
        # 遍历openlist中的每一个节点
        for entry in openlist.values():
            # 如果fast为空，则将当前节点赋值给fast
            if fast is None:
                fast = entry
            # 如果当前节点的f_cost小于fast的f_cost，则将当前节点赋值给fast
            elif fast.f_cost > entry.f_cost:
                fast = entry
        # 返回f_cost最小的节点
        return fast

    openlist = {}
    # 定义两个字典，分别存放待搜索的节点和已搜索过的节点
    closedlist = {}
    # 定义一个SearchEntry类，存放节点信息
    location = SearchEntry(source[0], source[1], 0.0)
    dest = SearchEntry(dest[0], dest[1], 0.0)
    # 将起始节点加入openlist
    openlist[source] = location

    while True:
        # 获取openlist中距离起点最近的点
        location = getFastPosition(openlist)
        # 如果没有找到有效的路径
        if location is None:
            # not found valid path
            print("can't find valid path")
            break;

        # 如果找到了终点
        if location.x == dest.x and location.y == dest.y:
            break

        # 将当前点加入到closedlist中
        closedlist[location.getPos()] = location
        # 将当前点从openlist中移除
        openlist.pop(location.getPos())
        # 遍历当前点的相邻点，将其加入到openlist中
        addAdjacentPositions(map, location, dest, openlist, closedlist)

    return location

# 定义一个函数，获取第一个步骤和距离
def getFirstStepAndDistance(location):
    # 初始化距离为0
    distance = 0
    # 将location赋值给tmp
    tmp = location
    # 当location的pre_entry不为空时
    while location.pre_entry is not None:
        # 距离加1
        distance += 1
        # 将location赋值给tmp
        tmp = location
        # 将location的pre_entry赋值给location
        location = location.pre_entry
    # 返回tmp的x坐标，y坐标和距离
    return (tmp.x, tmp.y, distance)

def getPosInRange(location, range):
    '''获取距离目的地距离为range的位置'''
    tmp = location
    while location.pre_entry is not None:
        if range == 0:
            break
        location = location.pre_entry
        tmp = location
        range -= 1

    return (tmp.x, tmp.y)

# 定义一个函数，计算给定地图上source和dest之间的A*距离
def getAStarDistance(map, source, dest):
    # 使用A*搜索算法获取source和dest之间的路径
    location = AStarSearch(map, source, dest)
    # 如果找到了路径
    if location is not None:
        # 获取第一步和距离
        _, _, distance = getFirstStepAndDistance(location)
    else:
        # 如果没有找到路径，则距离为None
        distance = None
    # 返回距离
    return distance
